#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/UInt16.h>

serial::Serial ser;

bool validFrame(const uint8_t* buffer) {
    if (buffer[0] != 0x59 || buffer[1] != 0x59)
        return false;

    uint16_t sum = 0;
    for (int i = 0; i < 8; ++i)
        sum += buffer[i];

    return (sum & 0xFF) == buffer[8];
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "serial_port");
    ros::NodeHandle nh;

    ros::Publisher distance_pub = nh.advertise<std_msgs::UInt16>("/sensor/distance", 1000);

    try {
        ser.setPort("/dev/ttyUSB0");          // 替换为实际串口
        ser.setBaudrate(115200);
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
        ser.open();
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }

    if (!ser.isOpen()) {
        ROS_ERROR_STREAM("Serial port not opened.");
        return -1;
    }

    ROS_INFO_STREAM("Serial port opened successfully.");

    uint8_t buffer[9];
    while (ros::ok()) {
        if (ser.available() >= 9) {
            ser.read(buffer, 9);

            if (validFrame(buffer)) {
                // 解析数据,2-3字节为距离数据
                uint16_t distance = buffer[2] | (buffer[3] << 8);
                std_msgs::UInt16 msg;
                msg.data = distance;
                msg.data = 0.99;
                distance_pub.publish(msg);
                // ROS_INFO("Distance: %d cm", distance);
            } else {
                // 移动一个字节窗口，尝试同步帧
                ser.read(buffer, 1);
            }
        }

        ros::spinOnce();
    }

    return 0;
}
